
/*
* core one, cooperator.
* proc message.
*/

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "rp2040.h"
#include "drvs/i2c.h"
#include "drvs/gpio.h"

#include "pico.h"
#include "pico/multicore.h"

#include "umsg.h"
#include "pmdio.h"

/* only for core-1, through Inter-processor FIFOs (Mailboxes) */
static inline void umsg_cop_send( umsg_t * pmsg )
{ 
    pmsg->data[0] = pmsg->type | 0x80;
    pmsg->mlen = pmsg->offs;
    multicore_fifo_push_blocking( (uint32_t)(intptr_t)pmsg );
    return;
}


/*
 input : addr(u32), cnt(u8)
output : value ... (u32)
*/
int cop_rreg( umsg_t * pmsg )
{
	volatile uint32_t * pu32;
	int cnt;

	/**/
	pu32 = (volatile uint32_t *)umsg_get_ptr( pmsg );
	cnt = (int)umsg_get_u8( pmsg );


	if ( (cnt > 16) || (cnt < 1) ) {
		/* error */
		umsg_free( pmsg );
		return 0;
	}

	/**/
	umsg_reset_ofs( pmsg );
	for ( int i=0; i<cnt; i++ ) {
		umsg_add_u32( pmsg, pu32[i] );
	}

	/* 继承 type, 不需要修改. */
	umsg_cop_send( pmsg );
	return 0;
}



static uint32_t p1_ticks = 0;

void  SysTick_P1_Handler( void )
{
	p1_ticks += 1;
}


int cop_common_cmd( umsg_t * pmsg )
{
	uint8_t subcmd;

	/**/
	subcmd = umsg_get_u8( pmsg );

	switch ( subcmd ) {

	case 0x0:
		/* echo */
		pmsg->offs = pmsg->mlen;
		umsg_cop_send( pmsg );
		break;
		
	case 0x1:
		cop_rreg( pmsg );
		break;

	case 0xFE:
		umsg_reset_ofs( pmsg );
		umsg_add_u32( pmsg, p1_ticks );
		umsg_cop_send( pmsg );
		break;

	case 0xff:
		umsg_reset_ofs( pmsg );
		umsg_add_u32( pmsg, __get_PRIMASK() );
		umsg_cop_send( pmsg );
		break;

	default:
		umsg_free( pmsg );
		break;
	}

	return 0;
}


int cop_mdio_cmd( umsg_t * pmsg )
{
	uint8_t subcmd;
	uint8_t phyadr;
	uint8_t devtyp;
	uint16_t data;
	uint32_t rult;

	/**/
	subcmd = umsg_get_u8( pmsg );

	switch ( subcmd ) {
	case 0:		/* init */
		mdio_intf_init( 10, 12, 20 );

		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, 0 );
		break;

	case 1:		/* address */
		phyadr = umsg_get_u8( pmsg );
		devtyp = umsg_get_u8( pmsg );
		data = umsg_get_u16( pmsg );
		mdio_address( phyadr, devtyp, data );

		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, 0 );
		break;

	case 2:		/* write */
		phyadr = umsg_get_u8( pmsg );
		devtyp = umsg_get_u8( pmsg );
		data = umsg_get_u16( pmsg );
		mdio_write( phyadr, devtyp, data );

		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, 0 );
		break;

	case 3:		/* read */
		phyadr = umsg_get_u8( pmsg );
		devtyp = umsg_get_u8( pmsg );
		mdio_read( phyadr, devtyp, &rult );
		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, 0 );
		umsg_add_u32( pmsg, rult );
		break;

	case 4:		/* post read */
		phyadr = umsg_get_u8( pmsg );
		devtyp = umsg_get_u8( pmsg );
		mdio_postrd( phyadr, devtyp, &rult );
		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, 0 );
		umsg_add_u32( pmsg, rult );
		break;

	default:
		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, 0x1122 );
		break;
	}

	/**/
	umsg_cop_send( pmsg );
	return 0;
}



int cop_iic_cmd( umsg_t * pmsg )
{
	int iret;
	i2c_inst_t * pi2c = i2c0;
	uint8_t subcmd;
	uint8_t addr;
	uint8_t tary[32];
	int tlen;
	
	/**/
	subcmd = umsg_get_u8( pmsg );

	switch ( subcmd )   {

	case 0:
		gpio_pull_up( 24 );
		gpio_pull_up( 25 );
		gpio_set_input_enabled( 24, true );
		gpio_set_input_enabled( 25, true );
		gpio_set_input_hysteresis_enabled( 24, true );
		gpio_set_input_hysteresis_enabled( 25, true );
		gpio_set_slew_rate( 24, GPIO_SLEW_RATE_SLOW );
		gpio_set_slew_rate( 25, GPIO_SLEW_RATE_SLOW );
		gpio_set_function( 24, GPIO_FUNC_I2C );
		gpio_set_function( 25, GPIO_FUNC_I2C );

		/**/
		i2c_init( pi2c, 300000 );
		break;

	case 1:
		addr = umsg_get_u8( pmsg );
		tlen = umsg_get_u8( pmsg );
		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );
		
		/**/
		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, iret );
		memcpy( umsg_curr(pmsg), tary, tlen );
		umsg_skip( pmsg, tlen );
		break;

	case 2:
		addr = umsg_get_u8( pmsg );
		tlen = umsg_get_u8( pmsg );
		iret = i2c_write_blocking( pi2c, addr, umsg_curr(pmsg), tlen, false );

		/**/
		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, iret );
		break;

	case 3:
		/* write addr, restart read */
		addr = umsg_get_u8( pmsg );
		tlen = umsg_get_u8( pmsg );
		tary[0] = umsg_get_u8( pmsg );
		iret = i2c_write_blocking( pi2c, addr, tary, 1, true );
		if ( iret != 0 ) {
			umsg_reset_ofs( pmsg );
			umsg_add_i32( pmsg, iret );
			break;
		}

		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );
		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, iret );
		memcpy( umsg_curr(pmsg), tary, tlen );
		umsg_skip( pmsg, tlen );
		break;

	default:
		umsg_reset_ofs( pmsg );
		umsg_add_i32( pmsg, 0x1124 );
		break;
	}

	umsg_cop_send( pmsg );
	return 0;
}


int cop_spi_cmd( umsg_t * pmsg )
{
	return 0;
}


void core1_main( void )
{
	umsg_t * pmsg;

	printf("hello, from, %d\n", sio_hw->cpuid );


#if 0
	/* I2C */
	gpio_pull_up( 0 );
	gpio_pull_up( 1 );
	gpio_set_input_enabled( 0, true );
	gpio_set_input_enabled( 1, true );
	gpio_set_input_hysteresis_enabled( 0, true );
	gpio_set_input_hysteresis_enabled( 1, true );
	gpio_set_slew_rate( 0, GPIO_SLEW_RATE_SLOW );
	gpio_set_slew_rate( 1, GPIO_SLEW_RATE_SLOW );
	gpio_set_function( 0, GPIO_FUNC_I2C );
	gpio_set_function( 1, GPIO_FUNC_I2C );
#endif


	/**/
	SysTick_Config( 9000000 );

	while ( 1 ) {

		/**/
		pmsg = (umsg_t *)(uintptr_t)multicore_fifo_pop_blocking();
		pmsg->type = pmsg->data[0] & 0x3F;
		umsg_reset_ofs( pmsg );

		// printf( "pmsg, %u: %p, %u, %u\n", cnt, pmsg, pmsg->type, pmsg->mlen );
		
		switch ( pmsg->type ) {
		
		case 0x0:	/* common cmds */
			cop_common_cmd( pmsg );
			break;

		case 1:		/* mdio */
			cop_mdio_cmd( pmsg );
			break;

		case 2: 	/* i2c */
			cop_iic_cmd( pmsg );
			break;

		case 3: 	/* spi */
			cop_spi_cmd( pmsg );
			break;

		default:
			umsg_reset_ofs( pmsg );
			umsg_add_i32( pmsg, 0x5555 );
			umsg_cop_send( pmsg );
			break;
		}

	}
}


